/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "J2B2OccuSLAM.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "RGBImage.h"       ///< RGBImage handling
int main(void){
		int Nx,Ny;
		J2B2OccuSLAM slammer(50,50,0.05,Grid::TCoord2D(0,0),0.6,0.6,181);
		slammer.getMapDim(Nx,Ny);
		RGBUtil::RGBImage rgb(Nx,Ny);
		
		///GLut imagegui
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		 /// Initialize the debugging module
		debugInit();
		debugSetGlobalDebugLvl(3);
		debugSetMode(DEBUG_MODE_TIMESTAMP_HH_MM_SS_MS | 
						DEBUG_MODE_FORMAT_FILE_LINE | 
						DEBUG_MODE_IMMEDIATE_FLUSH);
		
		slammer.connectToRobot("asrobo.hut.fi", 50002, "ASRobo_real");
		float x,y,a;
		int cnt=0;
		Grid::TPixel p;
		while(1){
			slammer.getPosition(x, y,a);	
			fprintf(stderr,"Pose now: (%.2f,%.2f,%.2f)\n",x,y,a);
			
			rgb.set(slammer.getMapDataPtr(), 255.0, 255.0,255.0);
			p = slammer.toPixel(x,y);
			rgb.setColor(0,0,255);
			rgb.setFilledCircle(p.x,p.y,0.25,0.05);
			rgb.setCoordinateFrame(p.x, p.y, a, 1, 0.05);
			displayImage(0,rgb.getDataPtr(),Ny,Nx);
			ownSleep_ms(200);
			cnt++;
			//if(cnt%100==0) slammer.reset();	
		}
	return 0;
}